#include "gpio.h"
#include "pid.h"
#include "math.h"

void PID_Update(PID_t *p)
{
    p->error1 = p->error0;
    p->error0 = p->target - p->actual;
    
    if(p->ki != 0)
    {
     p->errorInt += p->error0;
    }
    else
    {
        p->errorInt = 0;
    }
    if(p->errorInt > p->outmax) p->errorInt = p->outmax;
    if(p->errorInt < p->outmin) p->errorInt = p->outmin;
    p->out = p->kp * p->error0 + p->ki * p->errorInt + p->kd * (p->error0 - p->error1);
    if(p->out > p->outmax){p->out = p->outmax;}
    if(p->out < p->outmin){p->out = p->outmin;}
   
}

// static inline float clampf(float x, float lo, float hi)
// {
//     return (x < lo) ? lo : (x > hi) ? hi : x;
// }

// /* 等价写法：保持 errorInt 语义为“∫e·dt” */
// void PID_Update_dt(PID_t *p, float dt_s)
// {
//     /* dt 防护 */
//     if (dt_s <= 0.0f)  dt_s = 0.001f;
//     if (dt_s >  0.05f) dt_s = 0.001f;

//     /* 误差更新（沿用你的命名） */
//     float e_prev = p->error0;
//     p->error1 = p->error0;
//     p->error0 = p->target - p->actual;
//     float e = p->error0;

//     /* P 与 D（D 用 de/dt，避免随采样频率变化） */
//     float pTerm = p->kp * e;
//     float dTerm = p->kd * (e - e_prev) / dt_s;

//     /* 先用当前积分形成未饱和输出，判断是否允许继续积分 */
//     float iTerm_now = p->ki * p->errorInt;           /* errorInt 仍为“∫e·dt” */
//     float u_unsat   = pTerm + iTerm_now + dTerm;

//     int can_integrate =
//        ((u_unsat > p->outmin && u_unsat < p->outmax) ||   /* 未饱和可积分 */
//         (u_unsat >= p->outmax && e < 0.0f) ||             /* 上饱和且误差要把它拉回 */
//         (u_unsat <= p->outmin && e > 0.0f));              /* 下饱和且误差要把它推回 */

//     /* 条件积分：保持 errorInt 的语义（∫e·dt） */
//     if (p->ki != 0.0f) {
//         if (can_integrate) {
//             p->errorInt += e * dt_s;
//         }
//     } else {
//         p->errorInt = 0.0f;
//     }

//     /* 抗积分饱和：把 iTerm 限在对当前 P、D 合理的窗口内，并回写到 errorInt */
//     float iTerm = p->ki * p->errorInt;
//     float i_max = p->outmax - pTerm - dTerm;
//     float i_min = p->outmin - pTerm - dTerm;
//     if (i_max < i_min) { float t = i_max; i_max = i_min; i_min = t; }

//     if (iTerm > i_max) {
//         iTerm = i_max;
//         if (p->ki != 0.0f) p->errorInt = iTerm / p->ki;   /* 保持语义一致 */
//     } else if (iTerm < i_min) {
//         iTerm = i_min;
//         if (p->ki != 0.0f) p->errorInt = iTerm / p->ki;
//     }

//     /* 最终输出并限幅 */
//     float u = pTerm + iTerm + dTerm;
//     p->out = clampf(u, p->outmin, p->outmax);
// }

// /* 兼容旧调用：无 dt 版本（默认当 1ms） */
// #ifndef PID_DEFAULT_DT_S
// #define PID_DEFAULT_DT_S 0.001f
// #endif

// void PID_Update(PID_t *p)
// {
//     PID_Update_dt(p, PID_DEFAULT_DT_S);
// }